/*
 * File: BinaryInterfacePoseStruct.h
 * Created On: 2020-04-11
 * Copyright © 2020 Bosch Rexroth AG
*/

#include "ros/ros.h"
#include "nav_msgs/Odometry.h"
#include "std_msgs/String.h"
#include "msg2tcp/BinaryInterfaceServer.h"
#include "msg2tcp/OdomServer.h"
#include "tf/transform_datatypes.h"
//Open the socket

OdomServer::OdomServer(int port): port_(port), odom_number_(0){
  server_ = new BinaryInterfaceServer(port);
}


void OdomServer::updateOdom(const OdomMessage odom_message) {
  //odom_number_++;
  OdomMessage data = odom_message;
  //data.odom_number = odom_number_;
  server_->write(&data, sizeof(data)); //  server_->write(&data, sizeof(data));
}


//Broadcast to all clients the odometry data 
uint16_t currentOdoNum {0};
void odomCallback(const nav_msgs::Odometry::ConstPtr& msg, OdomServer* pToOdomServer)
{
  ROS_INFO("trigger");
  const std::chrono::duration<double> unixTimePoint = std::chrono::system_clock::now() - std::chrono::system_clock::time_point ();
  OdomMessage odom_message;
  odom_message.odom_number = ++currentOdoNum;
  odom_message.timestamp = unixTimePoint.count();  
  //odom_message.timestamp = msg->header.stamp.toSec();
  odom_message.x = msg->pose.pose.position.x;
  odom_message.y = msg->pose.pose.position.y;
  odom_message.yaw = tf::getYaw(msg->pose.pose.orientation);
  odom_message.velocity_set = false;
  //check time
  std::cout << odom_message.timestamp << std::endl;
  pToOdomServer->updateOdom(odom_message);
}

int main(int argc, char **argv)
{
	ros::init(argc, argv, "odom2tcp_node");

	ros::NodeHandle n("~");


    int port = 9091;
    std::string odom_topic = "odom";
    // std_msgs::String ip;

    if (n.getParam("port", port))
    {
      ROS_INFO("Got 'port' param: %d", port);
    }
    else
    {
      ROS_ERROR("Failed to get param 'port'");
    }

    if (n.getParam("odom_topic", odom_topic))
    {
      ROS_INFO("Got 'odom_topic' param: %s", odom_topic.c_str());

    }
    else
    {
      ROS_ERROR("Failed to get param 'odom_topic'");

    }

    // OdomServer* odom_server_ptr = newOdome(port);
    OdomServer* pToOdomServer = new OdomServer(port);

    //Subscribe to odom topic
    ros::Subscriber sub = n.subscribe<nav_msgs::Odometry> (odom_topic, 1000, boost::bind(&odomCallback, _1, pToOdomServer) );
    //Open socket of choice

    ros::spin();


    return 0;

}


